import radar_data
from matplotlib import pyplot as plt
import numpy as np

print(len(radar_data.range1))

print(radar_data.angle_max/radar_data.angle_increment)

data = np.array(radar_data.range1)
idata = 1/data

ddata = []
# for i in range(-1,359):
#     ddata.append(abs(data[i]-data[i+1]))

# 相关运算，并可消除位移
def relate(arr, kernel):
    length = len(arr)
    start = - int(len(kernel)/2)
    res = [] 
    for i in range(start, length + start):
        value = 0
        for k in range(len(kernel)):
            id = i + k
            if id >= length:
                id -= length 
            value += arr[id] * kernel[k]
        res.append(abs(value))   
    return res

# 环形位移
def ring_move(arr,offset):
    if offset >= 0:
        for i in range(offset):
            arr.insert(len(arr),arr[0])
            arr.remove(arr[0])
    else:
        for i in range(-offset):
            arr.insert(0,arr.pop())

def adapt_thres(arr,thres):
    km = [0.2,0.2,0.2,0.2,0.2]
    mean = relate(arr,km)
    map = []
    for i in range(len(arr)):
        if arr[i] > thres and arr[i] > mean[i]:
            map.append(-0.1)
        else:
            map.append(0.0)
    return map

        

k1 = [1,2,-7,2,1]
k2 = [0.2,0.2,-0.8,0.2,0.2]
km = [0.2,0.2,0.2,0.2,0.2]
ddata = relate(idata, k1)
jdata = adapt_thres(ddata, 0.09)

# ring_move(ddata,2)

# print(np.array(ddata)*100)
plt.plot(range(360),data)
plt.plot(range(360),ddata,'r')
plt.plot(range(360),jdata,'g')
plt.show()